#include <hidef.h> /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */
#include "PAP_defs.h"
#include "Services_QG.h"
#include "Sensor_Data_Handler.h"
#include "Button_Handler.h"

#pragma CONST_SEG CONSTANTS
// This segment stores the calibration data, and we need to tie
// the constants to a fixed physical memory address to ensure
// that the proper value is read.
const uint16 u16_ADCX_10b_zero_offset @0x0000E000 = ADC_10_BIT_ZERO_OFFSET;
const uint16 u16_ADCY_10b_zero_offset @0x0000E002 = ADC_10_BIT_ZERO_OFFSET;
const uint16 u16_ADCZ_10b_zero_offset @0x0000E004 = ADC_10_BIT_ZERO_OFFSET;

#pragma DATA_SEG MY_ZEROPAGE
// Time critical variables declared here, as long there is RAM left
// in the zeropage, the application should use it.
// Global variables because they are used inside interrupts
uint8 u8scheduler_counter;
uint8 u8scheduler_flag;
uint8 u8sci_request;
uint8 u8accel_x;
uint8 u8accel_y;
uint8 u8accel_z;
uint8 u8speed;
uint8 u8battery;
uint8 u8buttons;
uint8 u8recalibration_flag;
uint8 u8AccelStatus;
uint8 u8FreeFall;

#pragma DATA_SEG RAM
// Remaining variables declared here
// Global variables because they are used inside interrupts
                           
extern uint16 u16adc_new_x_offset;
extern uint16 u16adc_new_y_offset;
extern uint16 u16adc_new_z_offset;

void main(void) 
{
    uint8 u8tempdata;
    uint8 u8FlashOpsStatus;
       
    EnableInterrupts; /* enable interrupts */

    // Initialize recalibration flag as 0
    u8recalibration_flag = RECALIBRATION_NOT_REQUESTED;

    // Initialize the system options, watchdog enabled, 256ms, defaults
    vfnSystemOptionsInit ();

    // Configure Clock source, internal source, Bus = 8MHz, trimmed from flash
    vfnICS_Configure ();
        
    // Enable flash programming by configuring the clock at 200khz
    vfnFlashConfigureClock ();
    
    // Protect flash pages between 0xE200 and 0xFFFF without vector redirection
    // The first page (0xE000 - 0xE1FF) stays unprotected to allow writes
    vfnFlashProtect ();

    // Initialize ADC, 10 bits, single conversion, long sample time
    vfnSensor_ADC_Config ();
    
    // Test acceletometer
   	u8AccelStatus = u8Sensor_TestAccel();
    
    // Filter coefficient initialization
    vfnFilterInit();
    
    // Load the calibration parameters from non-volatile memory (flash)
    vfnSensor_LoadCalibrationValues ();

    // Configure the button GPIO pins
    vfnButton_GPIO_Setup ();

    // Initialize the button state machine
    vfnButton_StateInit ();
    
    // Initialize system time base at 8ms with interrupts enabled
    vfnSystemTimeBaseSetup ();
    
    
    vfnSCI_SetBaudRate (SCI_38400BAUD);
    vfnSCI_Init ();

    for(;;) 
    {
        /* This condition will be used very little, ideally, only once 
           per flight */
        if (u8recalibration_flag == RECALIBRATION_REQUESTED)
        {
        	DisableInterrupts;
        	u8recalibration_flag = RECALIBRATION_NOT_REQUESTED;
        	
        	u8FlashOpsStatus = 0;
        	
        	if( (u16adc_new_x_offset > ADC_MAX_CAL_OFFSET) || (u16adc_new_x_offset < ADC_MIN_CAL_OFFSET) )
        	{
				u8FlashOpsStatus |= 0x80;        		
        	}
        	if( (u16adc_new_y_offset > ADC_MAX_CAL_OFFSET) || (u16adc_new_y_offset < ADC_MIN_CAL_OFFSET) )
        	{
				u8FlashOpsStatus |= 0x80;        		
        	}
        	if( (u16adc_new_z_offset > ( ADC_MAX_CAL_OFFSET + 10 )) || (u16adc_new_z_offset < ( ADC_MIN_CAL_OFFSET - 10 )) )
        	{
				u8FlashOpsStatus |= 0x80;        		
        	}
        	
        	if(!u8FlashOpsStatus)
        	{
	            /* Flash writing routine */
	            
	            // The erasing procedure takes about 20ms, so it will interfere
	            // with the application, but it should be called so seldomly that
	            // it should not be a problem
	            
	            u8FlashOpsStatus = (u8FlashErase (0xE000));
	            u8FlashOpsStatus = u8FlashOpsStatus<<1;
	            	            
	            // Theoretically the following flash writes should take about 50us
	            // each, so the whole flash re-write should take about 21ms at most.
	            u8tempdata = (uint8)(u16adc_new_x_offset>>8);
	           	u8FlashOpsStatus = u8FlashProgramByte (0xE000, u8tempdata);
	            u8FlashOpsStatus = u8FlashOpsStatus<<1;
	            
	            u8tempdata = (uint8)(u16adc_new_x_offset);
	            u8FlashOpsStatus |= u8FlashProgramByte (0xE001, u8tempdata);
	            u8FlashOpsStatus = u8FlashOpsStatus<<1;
	            	            
	            u8tempdata = (uint8)(u16adc_new_y_offset>>8);
	            u8FlashOpsStatus |= u8FlashProgramByte (0xE002, u8tempdata);
				u8FlashOpsStatus = u8FlashOpsStatus<<1;
	            	            
	            u8tempdata = (uint8)(u16adc_new_y_offset);
	            u8FlashOpsStatus |= u8FlashProgramByte (0xE003, u8tempdata);
	            u8FlashOpsStatus = u8FlashOpsStatus<<1;
	            
	            u8tempdata = (uint8)(u16adc_new_z_offset>>8);
	            u8FlashOpsStatus |= u8FlashProgramByte (0xE004, u8tempdata);
	            u8FlashOpsStatus = u8FlashOpsStatus<<1;
	            
	            u8tempdata = (uint8)(u16adc_new_z_offset);
	            u8FlashOpsStatus |= u8FlashProgramByte (0xE005, u8tempdata);
	            	            
	            vfnSensor_LoadCalibrationValues();
	        
	        }
	            
           	vfnSCI_SendByte (u8FlashOpsStatus); //If(u8FLashOpsStatus) = error
    		while(!u8SCI_TxBufferEmpty());           
    		EnableInterrupts;
        }
    
        // The following two conditions are the task scheduler
        // proper, which will only be interrupted in case an SCI
        // request is incomming
        if (u8scheduler_flag == TASK_SENSORS)
        {
            // Call the Sensor Data Handler and get the sensor data
            // according to the configuration parameters (i.e. filtered,
            // unfiltered, etc.)
            u8FreeFall = 0;
            
            u8accel_x = u8Sensor_GetAccelX ();
            
            u8accel_y = u8Sensor_GetAccelY ();
            
            u8accel_z = u8Sensor_GetAccelZ ();
            
            if(u8FreeFall == ( SENSOR_FREE_FALL_X | SENSOR_FREE_FALL_Y | SENSOR_FREE_FALL_Z ))
            {
            	u8accel_x = 128;
            	u8accel_y = 128;
            	u8accel_z = 64;
            }
            
            u8speed = u8Sensor_GetSpeed ();
            
            u8battery = u8Sensor_GetBatteryLevel ();
            
            // Reset scheduler flag
            u8scheduler_flag = 0;    
        }
        if (u8scheduler_flag == TASK_BUTTONS)
        {
            // Read if any new buttons have been pressed.
            u8buttons = u8Button_Read () | ( u8buttons & 0x0F );
            //low nibble of u8Buttons is erased by SCI TX routine when pressed buttons have been
            //  sent to 1321
        
            // Reset scheduler flag
            u8scheduler_flag = 0;   
        }  

        __RESET_WATCHDOG(); /* feeds the dog */
        
        // Go to wait mode to save power.
        vfnSystemWait(); 
        
    } /* loop forever */
  /* please make sure that you never leave main */
}
